// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PoseTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Numerics
{
    using System;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Initializes a new instance of the PoseTest class.
    /// </summary>
    [TestClass]
    public class PoseTest
    {
        /// <summary>
        /// Make a random (valid) Pose.  Range of translational elements set by parameters, orientation is the full
        /// range of orientations supported by quaternions.
        /// </summary>
        /// <param name="minTranslationPerAxis"> The smallest (potentially negative) value allowed for translation along any axis.  </param>
        /// <param name="maxTranslationPerAxis"> The largest value allowed for translation along any axis.  </param>
        /// <returns>A random pose.</returns>
        public static Pose GenerateRandomPose(double minTranslationPerAxis, double maxTranslationPerAxis)
        {
            if (minTranslationPerAxis > maxTranslationPerAxis)
            {
                throw new ArgumentException("minTranslationPerAxis cannot be greater than maxTranslationPerAxis");
            }

            Random randomGen = new Random();

            double range = maxTranslationPerAxis - minTranslationPerAxis;

            Vector3 position = new Vector3(
                range * randomGen.NextDouble() + minTranslationPerAxis,
                range * randomGen.NextDouble() + minTranslationPerAxis,
                range * randomGen.NextDouble() + minTranslationPerAxis);

            Quaternion orientation = Quaternion.Normalize(
                new Quaternion(
                    randomGen.NextDouble(),
                    randomGen.NextDouble(),
                    randomGen.NextDouble(),
                    randomGen.NextDouble()));

            return new Pose(position, orientation);
        }

        /// <summary>
        /// Tests the vector pose constructor
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseVector3Constructor()
        {
            Pose pose = new Pose(new Vector3(1.0, 0.0, 0.0));
            Assert.IsTrue(
                pose.Position.X == 1.0 && pose.Position.Y == 0.0 && pose.Position.Z == 0.0 && pose.Orientation.X == 0.0
                && pose.Orientation.Y == 0.0 && pose.Orientation.Z == 0.0 && pose.Orientation.W == 1.0, 
                "Pose Vector constructor test failed " + pose.ToString());
        }

        /// <summary>
        /// Test component constructor
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseVectorQuaternionConstructor()
        {
            Pose pose = new Pose(new Vector3(1.0, 0.0, 0.0), new Quaternion(1.0, 2.0, 3.0, 4.0));
            Assert.IsTrue(
                pose.Position.X == 1.0 && pose.Position.Y == 0.0 && pose.Position.Z == 0.0 && pose.Orientation.X == 1.0
                && pose.Orientation.Y == 2.0 && pose.Orientation.Z == 3.0 && pose.Orientation.W == 4.0, 
                "Pose Vector Quaternion constructor test failed " + pose.ToString());
        }

        /// <summary>
        /// Test copy constructor
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseCopyConstructor()
        {
            Pose pose = new Pose(new Vector3(1.0, 0.0, 0.0), new Quaternion(1.0, 2.0, 3.0, 4.0));
            Pose result = new Pose(pose);
            Assert.IsTrue(
                result.Position.X == 1.0 && result.Position.Y == 0.0 && result.Position.Z == 0.0
                && result.Orientation.X == 1.0 && result.Orientation.Y == 2.0 && result.Orientation.Z == 3.0
                && result.Orientation.W == 4.0, 
                "Pose copy constructor test failed " + result.ToString());
        }

        /// <summary>
        /// Test the ToString operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseToString()
        {
            Pose pose = new Pose(new Vector3(1.0, 0.0, 0.0), new Quaternion(1.0, 2.0, 3.0, 4.0));
            string str = pose.ToString();

            // Not sure what the test should be
            Assert.IsTrue(str != null, "Pose tostring failed");
        }

        /// <summary>
        /// Test the equals method
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseEqualsMethod()
        {
            Vector3 vector = new Vector3(1.0, 2.0, 3.0);
            Quaternion quaternion = new Quaternion(1.0, 2.0, 3.0, 4.0);
            Pose leftSide = new Pose(vector, quaternion);
            Pose rightSide = new Pose(leftSide);
            Assert.IsTrue(
                leftSide.Equals(rightSide), 
                string.Format(
                    "Pose Equals method failed leftside = {0}, rightside = {1}", 
                    leftSide.ToString(), 
                    rightSide.ToString()));
            Assert.IsFalse(leftSide.Equals(vector), "Pose Equals method with non object failed ");
            Vector3 vector1 = new Vector3(5.0, 6.0, 7.0);

            // Need this next test to 100% code coverage.
            rightSide.Position = vector1;
            Assert.IsFalse(
                leftSide.Equals(rightSide), 
                string.Format(
                    "Pose Equals method with non-equal vector failed leftside = {0}, rightside = {1}", 
                    leftSide.ToString(), 
                    rightSide.ToString()));
        }

        /// <summary>
        /// Test the equals operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseEqualsOperator()
        {
            Vector3 vector = new Vector3(1.0, 2.0, 3.0);
            Quaternion quaternion = new Quaternion(1.0, 2.0, 3.0, 4.0);
            Pose leftSide = new Pose(vector, quaternion);
            Pose rightSide = new Pose(leftSide);
            Assert.IsTrue(
                leftSide == rightSide, 
                string.Format(
                    "Pose Equals operator failed leftside = {0}, rightside = {1}", 
                    leftSide.ToString(), 
                    rightSide.ToString()));
            Assert.IsFalse(
                leftSide != rightSide, 
                string.Format(
                    "Pose Not Equals operator failed leftside = {0}, rightside = {1}", 
                    leftSide.ToString(), 
                    rightSide.ToString()));
            Quaternion quat1 = new Quaternion(5.0, 6.0, 7.0, 8.0);

            // Need this next test to 100% code coverage.
            rightSide.Orientation = quat1;
            Assert.IsFalse(
                leftSide == rightSide, 
                string.Format(
                    "Pose Equals operator with non-equal quaternion failed leftside = {0}, rightside = {1}", 
                    leftSide.ToString(), 
                    rightSide.ToString()));
        }

        /// <summary>
        /// Tests the Hashcode method
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseGetHashCode()
        {
            Pose pose = new Pose();
            int result = pose.GetHashCode();
            Assert.IsTrue(result != 0, "Pose hash code is zero");
        }

        /// <summary>
        /// Tests conversion to/from Euler angles
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseEulerAngleConversion()
        {
            Random randGen = new Random();

            Vector3 randomEulerAngles = new Vector3(
                randGen.NextDouble() * MathConstants.TwoPI - Math.PI,
                randGen.NextDouble() * MathConstants.TwoPI - Math.PI,
                randGen.NextDouble() * MathConstants.TwoPI - Math.PI);

            Vector3 randomPosition = new Vector3(
                randGen.NextDouble(),
                randGen.NextDouble(),
                randGen.NextDouble());

            Pose pose = new Pose(randomPosition, randomEulerAngles);

            Vector3 convertedEulerAngles = pose.GetZYXEulerAngles();

            // now have two sets of Euler angles, which may correspond to same rotation even if they
            // are different.  Deal with this by converting to homogenous transforms and then testing
            Matrix4 randomRotationHt =
                HomogenousTransform.CreateFromRotationZ(randomEulerAngles.Z) *
                HomogenousTransform.CreateFromRotationY(randomEulerAngles.Y) *
                HomogenousTransform.CreateFromRotationX(randomEulerAngles.X);

            Matrix4 convertedRotationHt =
                HomogenousTransform.CreateFromRotationZ(convertedEulerAngles.Z) *
                HomogenousTransform.CreateFromRotationY(convertedEulerAngles.Y) *
                HomogenousTransform.CreateFromRotationX(convertedEulerAngles.X);

            Assert.IsTrue(randomRotationHt == convertedRotationHt, "Confirm that conversion to Euler angles and back was successful");
        }

        /// <summary>
        /// Test conversions between pose and homogenous 
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PoseHomogeneousConstructor()
        {
            Pose pose = new Pose(new Vector3(1.0, 0.0, 0.0), Quaternion.Normalize(new Quaternion(1.0, 2.0, 3.0, 4.0)));
            Matrix4 ht = HomogenousTransform.CreateFromPose(pose);
            Pose poseRoundTrip = new Pose(ht);

            Assert.AreEqual(pose.Orientation.X, poseRoundTrip.Orientation.X, MathConstants.ErrorEpsilon);
            Assert.AreEqual(pose.Orientation.Y, poseRoundTrip.Orientation.Y, MathConstants.ErrorEpsilon);
            Assert.AreEqual(pose.Orientation.Z, poseRoundTrip.Orientation.Z, MathConstants.ErrorEpsilon);
            Assert.AreEqual(pose.Orientation.W, poseRoundTrip.Orientation.W, MathConstants.ErrorEpsilon);
            Assert.AreEqual(pose.Position.X, poseRoundTrip.Position.X, MathConstants.ErrorEpsilon);
            Assert.AreEqual(pose.Position.Y, poseRoundTrip.Position.Y, MathConstants.ErrorEpsilon);
            Assert.AreEqual(pose.Position.Z, poseRoundTrip.Position.Z, MathConstants.ErrorEpsilon);
        }
    }
}
